using GeoJSON.Net.Feature;
using GeoJSON.Net.Geometry;
using GlobalData;
using Microsoft.EntityFrameworkCore;
using Microsoft.EntityFrameworkCore.Internal;
using Model.Core.Entity;
using Model.Core.Entity.Dbcontexts;
using Model.Core.MqModels;
using Model.Core.Services;
using Newtonsoft.Json;
using Newtonsoft.Json.Linq;
using RobotClient.Services;
using System.Threading;
namespace RobotClient.Controllers
{
    public class RobotJobControl : IHostedService
    {
        #region 构造
        public RobotJobControl(IDbContextFactory<DbcontextBase> contextFactory,
            IServiceProvider serviceProvider,
            JackDevCtrl jackDevCtrl,
            IConfiguration configuration,
            MqTopics globalMqTopics,
            ILogger<RobotJobControl> logger,
            LocalRobotInfo localRobotInfo)
        {
            ContextFactory = contextFactory;
            _Configuration = configuration;
            _GlobalMqTopics = globalMqTopics;
            _Logger = logger;
            LocalRobotInfo = localRobotInfo;
            _LocalMqClientCtrl = serviceProvider.GetService<LocalMqClientCtrl>();
            _CloudMqClientCtrl = serviceProvider.GetService<CloudMqClientCtrl>(); ;
            _NavHelper = serviceProvider.GetService<JxNavClient>();
            _JackDevCtrl = jackDevCtrl;
        }
        #endregion

        #region 属性定义

        Timer PubRobotJobStatusTimer { get; set; }

        //   public DbcontextBase _DbcontextBase { get; }
        public JxNavClient _NavHelper { get; set; }
        public IDbContextFactory<DbcontextBase> ContextFactory { get; }
        public JackDevCtrl _JackDevCtrl { get; }
        public IConfiguration _Configuration { get; }
        public MqTopics _GlobalMqTopics { get; }
        public ILogger _Logger { get; }
        public LocalRobotInfo LocalRobotInfo { get; }


        private object _moveLock = new object();
        private CancellationTokenSource _MoveTaskcts;
        private CancellationTokenSource _jobCts;
        private Task _currentMoveTask { get; set; }

        MqClientCtrl _LocalMqClientCtrl { get; set; }
        MqClientCtrl _CloudMqClientCtrl { get; set; }
        public JobRecord _CurrentJobStatus { get; set; } = null;

        List<JobNodeInfo> _JobInfoList { get; set; }
        /// <summary>
        /// 用于标识机器人任务的工作顺序
        /// </summary>
        private int _JobIndex { get; set; }


        /// <summary>
        /// 机器人移动到达位置状态
        /// </summary>
       // public bool SucessedAchievedStatus { get; set; }

        /// <summary>
        /// 导航位置到达事件
        ///// </summary>
        //public Action<bool, string> SucessedAchievedEvent { get; set; }
        public Action<string> ErrorMsgCallBack { get; set; }

        // public Action<double, double, double> RobotPointInfoCallBack { get; set; }
        public Action<double, double, double> RobotSpeedInfoCallBack { get; set; }

        /// <summary>
        /// 前后方向坐标
        /// </summary>
        public double Point_X { get; set; }
        /// <summary>
        /// 左右方向坐标
        /// </summary>
        public double Point_Y { get; set; }
        /// <summary>
        /// 角度
        /// </summary>
        public double Point_Angle { get; set; }
        /// <summary>
        /// 机器人运行速度
        /// </summary>
        public double Speed { get; set; }

        public bool BatteryCharge { get; set; }
        /// <summary>
        /// 当前机器人工作状态
        /// </summary>
        public RobotWorkStatusEnum _CurrentWorkStatusEnum { get; set; } = RobotWorkStatusEnum.Free;

        #endregion


        public void Init()
        {

        }


        #region 开始执行任务
        public void StartJob(string JobId, CancellationToken token = default)
        {
            _jobCts = CancellationTokenSource.CreateLinkedTokenSource(token);
            if (!ReadyToStartJob(JobId)) return;
            //任务确认执行后下发执行任务
            SendJob(_jobCts.Token);
        }
        #endregion

        private async Task PubRobotJobStatusFunc()
        {
            JObject jsondata = new JObject();
            string statusstr = string.Empty;
            switch (_CurrentWorkStatusEnum)
            {
                case RobotWorkStatusEnum.Running:
                    statusstr = "工作中";
                    break;
                case RobotWorkStatusEnum.Free:
                    statusstr = "空闲";
                    break;
                case RobotWorkStatusEnum.Charging:
                    statusstr = "充电中";
                    break;
            }
            string jsondata2 = string.Empty;
            if (_CurrentJobStatus != null)
                jsondata2 = JsonConvert.SerializeObject(_CurrentJobStatus);

            jsondata.Add("data", jsondata2);
            jsondata.Add("status", statusstr);
            await _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.Job_Status_Topic, jsondata.ToString());
        }
        void MqMsgRecvEventFunc()
        {
            _CloudMqClientCtrl.MsgReceivedEvent += async (a, b) =>
            {
                if (_GlobalMqTopics.Job_Ctrl_Topic.Equals(a))
                {
                    JobMqMsgDetail(b);
                }
                else if (_GlobalMqTopics.DoChargeTopic.Equals(a))
                {
                    await DocMoveReTreat();
                }
                else if (_GlobalMqTopics.RobotJobListInfoGet_Topic.Equals(a))
                {
                    try
                    {
                        string[] strings = _GlobalMqTopics.RobotJobListInfoGet_Topic.Split('/');
                        string RobotId = strings[1];
                        using (var _DbcontextBase = ContextFactory.CreateDbContext())
                        {
                            var joblist = _DbcontextBase.JobInfo.AsNoTracking().Where(a => a.RobotId == RobotId).ToList();
                            JArray senddata = new();

                            for (int i = 0; i < joblist.Count; i++)
                            {
                                var job = joblist[i];
                                JObject itemobject = new JObject
                            {
                                { "Id", job.Id },
                                { "RobotId", job.RobotId },
                                { "name", job.Name },
                                { "JobType", job.JobType }
                            };
                                senddata.Add(itemobject);
                            }
                            try
                            {
                                var json = JsonConvert.SerializeObject(senddata);
                                Task sendjobinfotask = _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.RobotJobListInfoPub_Topic, json);
                            }
                            catch (JsonException ex)
                            {
                                _Logger.LogError(ex, "JSON序列化异常");
                            }
                        }
                    }
                    catch (Exception ex)
                    {

                        _Logger.LogError($"获取任务列表异常 错误消息： {ex.Message}");
                    }

                }
                else if (_GlobalMqTopics.Move_Topic.Equals(a))
                {
                    JObject jsondata = JsonConvert.DeserializeObject<JObject>(b);
                    string command = jsondata["command"]?.ToString();
                    string direction = jsondata["type"]?.ToString();

                    lock (_moveLock)
                    {
                        if (command == "Start")
                        {
                            if (_currentMoveTask == null || _currentMoveTask.IsCompleted)
                            {
                                _MoveTaskcts = new CancellationTokenSource();
                                _currentMoveTask = new Task(() => SpeedControlTask(direction, _MoveTaskcts.Token), _MoveTaskcts.Token);
                                _currentMoveTask.Start();
                            }
                        }
                        else if (command == "Stop")
                        {
                            _MoveTaskcts?.Cancel();
                        }
                    }
                }
                else if (_GlobalMqTopics.MoveToPoint_Topic.Equals(a))
                {
                    //  Logger.LogInformation("接收到导航目标点消息 Topic:{Topic} Payload:{Payload}", a, b);
                    JObject jsondata = JsonConvert.DeserializeObject<JObject>(b);
                    double X = Convert.ToDouble(jsondata["x"]);
                    double Y = Convert.ToDouble(jsondata["y"]);
                    double Z = Convert.ToDouble(jsondata["z"]);
                    int type = Convert.ToInt32(jsondata["type"]);
                    //long MoveId = DateTime.UtcNow.Ticks;
                    PointInfo pointInfo = new PointInfo() { Name = string.Empty };
                    pointInfo.X = X;
                    pointInfo.Y = Y;
                    pointInfo.Angle = Z;
                    int movetype = 0;
                    CancellationTokenSource _jobCts = new CancellationTokenSource(TimeSpan.FromMinutes(10));
                    MoveToPoint(pointInfo, type, movetype, _jobCts.Token);
                }
                else if (_GlobalMqTopics.Map_UpdateJack.Equals(a))
                {
                    EditCarryMapDataFunc();
                }
                else if (_GlobalMqTopics.RobotPointListInfoGet_Topic.Equals(a))
                {
                    try
                    {
                        using (var _DbcontextBase = ContextFactory.CreateDbContext())
                        {
                            var robotinfo = _DbcontextBase.RobotInfo.Include(a => a.MapInfo).FirstOrDefault(a => a.RobotId == LocalRobotInfo.RobotId);
                            var PointList = _DbcontextBase.PointInfo.Where(a => robotinfo != null && robotinfo.MapInfo != null && a.MapId == robotinfo.MapInfo.MapId && a.PointType.ToLower() == "jack").ToList();
                            JArray jArray = new JArray();
                            foreach (var item in PointList)
                            {
                                JObject itemdata = new JObject();
                                itemdata.Add("id", item.Id.ToString());
                                itemdata.Add("name", item.Name);
                                jArray.Add(itemdata);
                            }
                            try
                            {
                                var json = JsonConvert.SerializeObject(jArray);
                                var task = _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.RobotPointListInfoPub_Topic, json);
                            }
                            catch (JsonException ex)
                            {
                                _Logger.LogError(ex, "JSON序列化异常");
                            }
                        }
                    }
                    catch (Exception ex)
                    {

                        _Logger.LogError($"获取顶升点位信息异常 错误消息： {ex.Message}");
                    }

                }
                else if (_GlobalMqTopics.RobotWheelCommandTopic.Equals(a))
                {
                    //处理轮子控制
                    JObject jsondata = JsonConvert.DeserializeObject<JObject>(b);
                    string command = jsondata["command"].ToString();
                    _NavHelper.SetWheelState(command);
                }
            }
                ;
            _LocalMqClientCtrl.MsgReceivedEvent += async (a, b) =>
            {
                if (MqTopics.Robot_Local_Status_Battery.Equals(a))
                {
                    JObject recvobj = JsonConvert.DeserializeObject<JObject>(b);
                    JObject keyValuePairs = new JObject();
                    BatteryCharge = Convert.ToBoolean(recvobj["IsCharge"]);
                    int num = Convert.ToInt32(recvobj["PowerInfo"]);
                    keyValuePairs.Add("ischarge", BatteryCharge);
                    keyValuePairs.Add("num", num);
                    if (_NavHelper.BatteryState.Ischarge == false && BatteryCharge == true)
                    {
                        Console.WriteLine($"[{DateTime.Now.ToString("yyyyMMdd HH:mm:ss:ff")}]:接受到充电信号！");
                    }
                    _NavHelper.BatteryState = (BatteryCharge, num);

                    await _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.RobotBatteryStatus_Topic, JsonConvert.SerializeObject(keyValuePairs)).ConfigureAwait(false);
                }

            };
        }



        private void SpeedControlTask(string direction, CancellationToken token)
        {

            double currentX = 0;
            double currentAngle = 0;
            while (!token.IsCancellationRequested)
            {
                if (Wheel_state != "remote")
                {
                    _MoveTaskcts.Cancel();
                    _NavHelper.SendMoveSpeed(0, 0);
                    break;
                }
                double step = 0.05;
                double maxSpeed = 0.3;
                // 根据方向类型调整对应轴
                if (direction == "up" || direction == "down")
                {
                    currentX = Math.Clamp(currentX + (direction == "up" ? step : -step), -maxSpeed, maxSpeed);
                }
                else if (direction == "left" || direction == "right")
                {
                    currentAngle = Math.Clamp(currentAngle + (direction == "right" ? -step : step), -maxSpeed, maxSpeed);
                }
                else
                {
                    currentX = 0;
                    currentAngle = 0;
                }

                _NavHelper.SendMoveSpeed(currentX, currentAngle);
                Thread.Sleep(50);
            }
        }

        Task CurrentJobTask { get; set; }
        void JobMqMsgDetail(string payload)
        {
            try
            {
                JObject jsondata = JsonConvert.DeserializeObject<JObject>(payload);
                string command = jsondata["command"].ToString();
                if (command.Equals("start_job"))
                {
                    string jobid = jsondata["jobid"]?.ToString();

                    try
                    {
                        if (string.IsNullOrEmpty(jobid))
                        {
                            string JobName = $"Job_{DateTime.Now.ToString("yyyyMMddHHmmss")}";
                            jobid = Guid.NewGuid().ToString();
                            using (var _DbcontextBase = ContextFactory.CreateDbContext())
                            {
                                _DbcontextBase.JobInfo.Add(new JobInfo()
                                {
                                    Id = Guid.Parse(jobid),
                                    Name = JobName,
                                    RobotId = _Configuration.GetSection("RobotInfo:RobotId").Value,
                                    JobType = "HocCarryJob"
                                });
                                var dataobj = jsondata["data"];
                                string startid = dataobj["start_id"]?.ToString();
                                string Endid = dataobj["end_id"]?.ToString();
                                _DbcontextBase.JobNodeInfo.AddRange(
                                    new JobNodeInfo()
                                    {
                                        Id = Guid.NewGuid(),
                                        jobid = Guid.Parse(jobid),
                                        ActionType = "align_with_rack",
                                        ActionData = startid,
                                        Name = "align_with_rack",
                                        ordernum = 1
                                    }, new JobNodeInfo()
                                    {
                                        Id = Guid.NewGuid(),
                                        jobid = Guid.Parse(jobid),
                                        ActionType = "to_unload_point",
                                        Name = "to_unload_point",
                                        ActionData = Endid,
                                        ordernum = 3
                                    }, new JobNodeInfo()
                                    {
                                        Id = Guid.NewGuid(),
                                        jobid = Guid.Parse(jobid),
                                        ActionType = "jack_up",
                                        Name = "jack_up",
                                        ActionData = startid,
                                        ordernum = 2
                                    }, new JobNodeInfo()
                                    {
                                        Id = Guid.NewGuid(),
                                        jobid = Guid.Parse(jobid),
                                        ActionType = "jack_down",
                                        Name = "jack_down",
                                        ActionData = startid,
                                        ordernum = 4
                                    }

                                );
                                _DbcontextBase.SaveChanges();
                            }
                        }
                        if (CurrentJobTask == null)
                        {
                            _jobCts = new CancellationTokenSource();
                            lock (_moveLock)
                            {
                                CurrentJobTask = Task.Factory.StartNew(() =>
                                            {
                                                using (var jobLock = new SemaphoreSlim(1, 1))
                                                {
                                                    jobLock.Wait(_jobCts.Token);
                                                    try
                                                    {
                                                        StartJob(jobid, _jobCts.Token);
                                                    }
                                                    finally
                                                    {
                                                        jobLock.Release();
                                                    }
                                                }
                                            }, _jobCts.Token);
                            }
                        }
                    }
                    catch (TaskCanceledException ex)
                    {
                        _Logger.LogWarning($"任务取消异常：{ex.Message}");
                    }
                    catch (OperationCanceledException ex)
                    {
                        _Logger.LogWarning($"操作取消异常：{ex.Message}");
                    }
                    catch (Exception ex)
                    {
                        _Logger.LogError(ex, "任务执行异常");
                    }

                }
                else if (command.Equals("stop_job"))
                {
                    Stop_Job(2);
                }

            }
            catch (Exception ex)
            {

                _Logger.LogError($"任务指令异常 错误消息： {ex.Message}");
            }

        }

        private async Task AddSubMqFunc()
        {
            // var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5));
            _Logger.LogInformation("开始订阅MQTT消息主题...");
            var cloudTopics = new[] {
                _GlobalMqTopics.Job_Ctrl_Topic,
                _GlobalMqTopics.RobotJobListInfoGet_Topic,
                _GlobalMqTopics.DoChargeTopic,
                _GlobalMqTopics.MoveToPoint_Topic,
                _GlobalMqTopics.Move_Topic,
                _GlobalMqTopics.Map_UpdateJack,
                _GlobalMqTopics.RobotPointListInfoGet_Topic,
                _GlobalMqTopics.RobotWheelCommandTopic
            };

            var localTopics = new[] {
                MqTopics.Robot_Local_Status_Battery
            };

            try
            {
                await BatchSubscribe(_CloudMqClientCtrl, cloudTopics);
                await BatchSubscribe(_LocalMqClientCtrl, localTopics);
                _Logger.LogInformation("MQTT主题订阅完成");
            }
            catch (Exception ex)
            {
                _Logger.LogError(ex, "MQTT主题订阅失败");
                throw;
            }
        }

        private async Task BatchSubscribe(MqClientCtrl client, IEnumerable<string> topics)
        {
            foreach (var topic in topics)
            {
                try
                {
                    await client.AddSubTopic(topic);
                    _Logger.LogInformation($"成功订阅主题：{topic}");
                }
                catch (OperationCanceledException)
                {
                    _Logger.LogWarning($"订阅主题 {topic} 操作超时");
                    throw;
                }
                catch (Exception ex)
                {
                    _Logger.LogError(ex, $"订阅主题 {topic} 时发生错误");
                    throw;
                }
            }
        }

        #region 执行当前节点动作
        private async Task SendJob(CancellationToken token)
        {
            try
            {
                //如果机器人不在工作状态，不接受 SendJob指令
                if (_CurrentWorkStatusEnum != RobotWorkStatusEnum.Running) return;
                if (_JobIndex >= _JobInfoList.Count)
                {
                    Console.WriteLine($"Send Job Index={_JobIndex} >= Current JobList Count{_JobInfoList.Count} Job Completed!");
                    Stop_Job(0);
                    return;
                }
                Task<(bool sucessed, string msg)> _robotindexjob = null;
                var JobItem = _JobInfoList[_JobIndex];
                if (JobItem.ActionType.ToLower().Equals("move"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 0, 0, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("movexy"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 1, 0, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("movexyinline"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 1, 3, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("moveangle"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 2, 0, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("motor"))
                {
                    _robotindexjob = MotorDevManage(JobItem.ActionData);
                }
                else if (JobItem.ActionType.ToLower().Equals("redlight"))
                {
                    _robotindexjob = Task.Factory.StartNew(() =>
                    {
                        RedLightManage(true);
                        Task.Delay(3000).Wait();
                        RedLightManage(false);
                        return (true, "sucessed");
                    });
                }
                else if (JobItem.ActionType.ToLower().Equals("spinmotor"))
                {
                    _robotindexjob = SpinMotorDevManage(JobItem.ActionData);

                }
                else if (JobItem.ActionType.ToLower().Equals("moveadvance"))
                {
                    _robotindexjob = DoMoveAdvanceFunc();

                }
                else if (JobItem.ActionType.ToLower().Equals("docharge"))
                {
                    _robotindexjob = DocMoveReTreat();
                }
                else if (JobItem.ActionType.ToLower().Equals("align_with_rack"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 0, 1, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("to_unload_point"))
                {
                    _robotindexjob = MoveToPoint(JobItem.ActionData, 0, 2, token);
                }
                else if (JobItem.ActionType.ToLower().Equals("jack_up"))
                {
                    _robotindexjob = _JackDevCtrl.Jack_Ctrl_Func(JackCtrlType.up);
                }
                else if (JobItem.ActionType.ToLower().Equals("jack_down"))
                {
                    _robotindexjob = _JackDevCtrl.Jack_Ctrl_Func(JackCtrlType.down);
                }

                var awaitresult = _robotindexjob?.GetAwaiter();

                if (awaitresult != null)
                {
                    awaitresult.Value.OnCompleted(() =>
                    {
                        var Result = awaitresult.Value.GetResult();
                        if (Result.sucessed)
                        {
                            _JobIndex++;
                            Task.Delay(20).Wait();
                            SendJob(token);
                        }
                        else
                        {
                            Stop_Job(1, Result.msg);
                        }
                    });
                }
            }
            catch (Exception ex)
            {

                _Logger.LogError(ex, $"Job execution failed after ms");
            }
        }




        #endregion


        public void EditCarryMapDataFunc()
        {
            try
            {
                using (var _DbcontextBase = ContextFactory.CreateDbContext())
                {
                    var robotinfo = _DbcontextBase.RobotInfo.Include(a => a.MapInfo).FirstOrDefault(a => a.RobotId == LocalRobotInfo.RobotId);
                    if (robotinfo != null && robotinfo.MapInfo != null) {
                        var TkdListInfo = _DbcontextBase.PointInfo.Where(a => a.MapId.Id == robotinfo.MapInfo.Id && a.PointType.ToLower() == "jack").ToList();
                        FeatureCollection featureCollection = new();
                        foreach (var item in TkdListInfo)
                        {
                            Feature feature = new(
                                new Point(new Position(item.Y, item.X)),
                                new Dictionary<string, object>
                                {
                                {"name",item.Name },
                                {"type", "34" },
                                {"mapOverlay", true},
                                {"subtype", "rack"},
                                {"dockingPointId",item.Id.ToString()},
                                {"yaw", (item.Angle / Math.PI * 180).ToString()}
                                },
                               item.Id.ToString()
                                );
                            featureCollection.Features.Add(feature);
                        }
            ;

                        string data = JsonConvert.SerializeObject(featureCollection);
                        _NavHelper.PatchMapApi(data);
                    }
                }
            }
            catch (Exception ex)
            {

                _Logger.LogError("更新顶升点位信息异常 错误消息： {ex.Message}", ex.Message);
            }




        }

        #region 停止任务
        /// <summary>
        /// 0 表示任务正常结束.1.表示有异常执行行为，需要主动结束 2.表示任务接收到任务指令结束
        /// </summary>
        /// <param name="num"></param>
        public void Stop_Job(int num, string Type = "")
        {
            lock (_moveLock)
            {
                _jobCts?.Cancel();
                _MoveTaskcts?.Cancel();
            }
            try
            {
                string StopType = string.Empty;
                switch (num)
                {
                    case 0:
                        StopType = "正常结束";
                        break;
                    case 1:
                        StopType = "任务异常结束";
                        _NavHelper.StopNav();
                        break;
                    case 2:
                        StopType = "任务指令结束";
                        _NavHelper.StopNav();
                        break;
                    case 3:
                        StopType = "任务结束";
                        _NavHelper.StopNav();
                        break;
                }
                _Logger.LogInformation($"停止导航任务=>停止类型 :{StopType}");
                if (_CurrentJobStatus != null)
                {
                    try
                    {

                        using (var _DbcontextBase = ContextFactory.CreateDbContext())
                        {
                            if (!string.IsNullOrEmpty(Type)) StopType += Type;
                            _CurrentWorkStatusEnum = RobotWorkStatusEnum.Free;
                            if (_CurrentJobStatus != null)
                            {
                                _CurrentJobStatus.StopType = StopType;
                                _CurrentJobStatus.StopTime = DateTime.Now;
                            }

                            if (_CurrentJobStatus?.JobType == "HocCarryJob")
                            {
                                var jobinfo = _DbcontextBase.JobInfo.FirstOrDefault(a => a.Id.ToString() == _CurrentJobStatus.JobId);
                                if (jobinfo != null)
                                {
                                    _DbcontextBase.JobInfo.Remove(jobinfo);
                                    _DbcontextBase.JobNodeInfo.RemoveRange(_DbcontextBase.JobNodeInfo.Where(a => a.jobid.ToString() == _CurrentJobStatus.JobId));
                                }
                            }
                            if (_CurrentJobStatus?.JobType == "StackerJob")
                            {
                                //任务结束后需要将旋转电机停止，
                                _ = SpinMotorDevManage("off");
                                //升降电机置顶
                                _ = MotorDevManage("up");
                            }
                            _ = _DbcontextBase.JobRecord.Update(_CurrentJobStatus);
                            _DbcontextBase.SaveChanges();
                            _Logger.LogInformation($"任务:{_CurrentJobStatus.JobName}:Id：{_CurrentJobStatus.JobId} Type：{_CurrentJobStatus.JobType}结束");
                        }

                    }
                    catch (Exception ex)
                    {

                        _Logger.LogError($"停止任务异常 错误消息： {ex.Message}");
                    }

                }
                if (CurrentJobTask != null)
                {
                    CurrentJobTask.Dispose();
                    CurrentJobTask = null;
                }
                _JobIndex = 0;
                _CurrentJobStatus = null;
            }
            catch (Exception ex)
            {
                _Logger.LogError($"停止任务异常 错误消息： {ex.Message}");
            }


        }
        #endregion

        #region 任务前准备工作
        /// <summary>
        /// 开始任务前的准备工作
        /// </summary>
        /// <returns></returns>
        public bool ReadyToStartJob(string JobId)
        {

            using (var _DbcontextBase = ContextFactory.CreateDbContext())
            {
                if (_CurrentJobStatus != null)
                {
                    Console.WriteLine($"Job Is Runing JobId ={_CurrentJobStatus.JobId} JobName = {_CurrentJobStatus.JobName}! Please Stop It Before");

                    return false;
                }

                var jobinfo = _DbcontextBase.JobInfo.ToList().Find(a => a.Id.ToString() == JobId);
                if (jobinfo == null)
                {
                    Console.WriteLine($"JobId {JobId} Is Empty!");
                    return false;
                }
                _JobInfoList = _DbcontextBase.JobNodeInfo.Where(a => a.jobid.ToString() == JobId).OrderBy(a => a.ordernum).ToList();

                if (_JobInfoList == null || _JobInfoList.Count == 0)
                {
                    Console.WriteLine($"Job Data In {JobId}  Is Empty!");
                    return false;
                }
                _CurrentJobStatus = new JobRecord();
                _CurrentJobStatus.StartTime = DateTime.Now;
                _CurrentJobStatus.StopTime = DateTime.Now.AddMinutes(5);
                _CurrentJobStatus.JobId = JobId;
                _CurrentJobStatus.JobName = jobinfo.Name;
                _CurrentJobStatus.Id = Guid.NewGuid();
                _CurrentJobStatus.StopType = "未结束";
                _CurrentJobStatus.JobType = jobinfo.JobType;
                _CurrentJobStatus.RobotId = _Configuration.GetSection("RobotInfo:RobotId").Value;
                _CurrentWorkStatusEnum = RobotWorkStatusEnum.Running;


                _DbcontextBase.JobRecord.Add(_CurrentJobStatus);
                _DbcontextBase.SaveChanges();
            }

            return true;
        }
        #endregion



        #region 导航

        #region 移动


        // private Guid _currentNavigationTaskId;

        /// <summary>
        /// 只移动到点
        /// </summary>
        public async Task<(bool, string)> MoveToPoint(string pointstr, int PointType, int movetype, CancellationToken token)
        {
            try
            {
                using (var _DbcontextBase = ContextFactory.CreateDbContext())
                {
                    var pointInfo = _DbcontextBase.PointInfo.AsNoTracking().FirstOrDefault(a => a.Id.ToString() == pointstr);
                    return await MoveToPoint(pointInfo, PointType, movetype, token);
                }
            }
            catch (Exception ex)
            {

                _Logger.LogError(ex, "MoveToPoint Failed");
                return (false, ex.Message);
            }
        }
        public async Task<(bool successed, string msg)> MoveToPoint(PointInfo pointInfo, int PointType, int movetype, CancellationToken token)
        {
            try
            {
                int maxRetries = 3;
                int retryCount = 0;

                while (retryCount < maxRetries)
                {

                    _Logger.LogInformation($"开始导航到点位 {{Name:{pointInfo.Name}, Id:{pointInfo.Id}}}");

                    var moveResult = await SendMoveCommandAsync(pointInfo, PointType, movetype);
                    if (!moveResult.success)
                    {
                        _Logger.LogError($"导航指令发送失败 {{Point:{pointInfo.Name}}}");
                        return (false, $"导航指令发送失败 {{Point:{pointInfo.Name}}}");
                    }
                    int moveId = moveResult.moveId ?? 0;
                    if (moveId <= 0)
                    {
                        _Logger.LogError($"导航指令发送失败，移动ID无效 {{Point:{pointInfo.Name}}}");
                        // return (false, $"导航指令发送失败 {{Point:{pointInfo.Name}}}"); ;
                    }
                    var completionResult = await WaitForNavigationCompletionAsync(moveId, token);

                    if (completionResult.success)
                    {
                        _Logger.LogInformation($"成功到达点位 {{Name:{pointInfo.Name}, Id:{pointInfo.Id}}}");
                        return (true, "sucessed");
                    }

                    _Logger.LogWarning($"导航超时，第{{retryCount}}次重试 {{Point:{pointInfo.Name}}}");
                    retryCount++;

                }

                _Logger.LogError($"导航失败，已达最大重试次数 {{Point:{pointInfo.Name}}}");
                return (false, $"导航失败，已达最大重试次数 {{Point:{pointInfo.Name}}}");
            }
            catch (OperationCanceledException)
            {
                _Logger.LogWarning($"导航任务被取消 Point:{pointInfo.Name}");
                return (false, $"导航任务被取消  Point: {pointInfo.Name} ");
            }
            catch (Exception ex)
            {
                _Logger.LogError(ex, $"导航过程中发生异常 {{Point:{pointInfo.Name}}}");
                return (false, $"导航过程中发生异常 {{Point:{pointInfo.Name}}}");
            }
        }

        private async Task<(bool success, int? moveId)> SendMoveCommandAsync(PointInfo pointInfo, int pointType, int moveType)
        {
            try
            {
                var data = _NavHelper.GetInputPointData(
                    Convert.ToDouble(pointInfo.X),
                    Convert.ToDouble(pointInfo.Y),
                    Convert.ToDouble(pointInfo.Angle),
                    pointType: pointType);

                var moveId = await _NavHelper.SendMovePoint((RobotMoveType)moveType, data);
                return (moveId > 0, moveId);
            }
            catch (Exception ex)
            {
                _Logger.LogError(ex, $"发送导航指令失败 {{Point:{pointInfo.Name}}}");
                return (false, null);
            }
        }

        private async Task<(bool success, string reason)> WaitForNavigationCompletionAsync(int moveid, CancellationToken cancellationToken)
        {
            try
            {
                const int pollingInterval = 200;
                using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(pollingInterval));

                try
                {
                    while (await timer.WaitForNextTickAsync(cancellationToken))
                    {
                        var stateResult = await _NavHelper.GetMovePlanState(moveid.ToString());
                        var stateData = JObject.Parse(stateResult);
                        string resulttype = stateData["state"]?.ToString().ToLower();
                        switch (resulttype)
                        {
                            case "moving":
                                continue;
                            case "succeeded":
                                return (true, "导航成功");
                            default:
                                return (false, $"计划Id:{moveid},错误类型:{resulttype} 导航异常：{stateData["fail_reason_str"]}");
                        }
                    }
                    return (false, "操作已取消");
                }
                catch (OperationCanceledException)
                {
                    return (false, "操作超时");
                }
            }
            catch (OperationCanceledException)
            {
                return (false, "操作超时");
            }
        }
        #endregion

        #region 后退
        /// <summary>
        /// 后退
        /// </summary>
        /// <returns></returns>
        public async Task<(bool successed, string msg)> DocMoveReTreat()
        {
            string speedstr = _Configuration.GetSection("MapNavOption:ChargeSpeed").Value;
            string distancestr = _Configuration.GetSection("MapNavOption:charge_distance").Value;
            string charge_timeoutstr = _Configuration.GetSection("MapNavOption:charge_timeout").Value;
            double speed = 0.03;
            double distance = -1;
            int charge_timeout = 15;
            if (!string.IsNullOrEmpty(speedstr))
            {
                speed = Convert.ToDouble(speedstr);
            }
            if (!string.IsNullOrEmpty(distancestr))
            {
                distance = Convert.ToDouble(distancestr);
            }
            if (!string.IsNullOrEmpty(charge_timeoutstr))
            {
                charge_timeout = Convert.ToInt32(charge_timeoutstr);
            }
            using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(30));
            //bool issucess = doChargeFunc(speed, distance, charge_timeout);
            bool issucess = await _NavHelper.StartMoveReTreat(speed, distance, charge_timeout, cts.Token);
            return (issucess, "successed");
        }
        /// <summary>
        /// 前进
        /// </summary>
        /// <returns></returns>
        public async Task<(bool successed, string msg)> DoMoveAdvanceFunc()
        {
            string speedstr = _Configuration.GetSection("MapNavOption:ChargeSpeed").Value;
            string distancestr = _Configuration.GetSection("MapNavOption:charge_distance").Value;
            string charge_timeoutstr = _Configuration.GetSection("MapNavOption:charge_timeout").Value;
            double speed = 0.03;
            double distance = -1;
            int charge_timeout = 15;
            if (!string.IsNullOrEmpty(speedstr))
            {
                speed = Convert.ToDouble(speedstr) * -1;
            }
            if (!string.IsNullOrEmpty(distancestr))
            {
                distance = Convert.ToDouble(distancestr);
            }
            if (!string.IsNullOrEmpty(charge_timeoutstr))
            {
                charge_timeout = Convert.ToInt32(charge_timeoutstr);
            }
            using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(30));
            bool issucess = await _NavHelper.StartMoveAdvance(speed, distance, charge_timeout, cts.Token);
            return (true, "successed");
        }

        #endregion

        //#region 注册导航
        //void InitNav()
        //{
        //    //IConfiguration localstr = _Configuration.GetSection("MapNavOption");
        //    //String ip = localstr.GetSection("HttpServerIp").Value;
        //    //int port = Convert.ToInt32(localstr.GetSection("HttpServerPort").Value);
        //    //string account = localstr.GetSection("Account").Value;
        //    //string pwd = localstr.GetSection("Pwd").Value;
        //    //string MapName = localstr.GetSection("MapName").Value;
        //    //_NavHelper = new JXNavHelper();
        //    //_NavHelper.InitNav(ip, port, account, pwd, MapName);


        //}
        //#endregion

        public string Wheel_state = "auto";
        #region 导航消息处理
        void NavMsgDetailFunc()
        {
            if (_NavHelper != null)
            {
                //机器人位置坐标
                _NavHelper.ActionPointInfo += (A, B, C) =>
                {
                    Point_X = A;
                    Point_Y = B;
                    Point_Angle = C;
                    JObject jsondata = new JObject();
                    jsondata["x"] = A;
                    jsondata["y"] = B;
                    jsondata["z"] = C;
                    _ = _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.RobotPointStatusTopic, jsondata.ToString());
                };

                _NavHelper.ActionWheelStateInfo += (A) =>
                {
                    JObject jsondata = JsonConvert.DeserializeObject<JObject>(A);
                    Wheel_state = jsondata["control_mode"].ToString();
                    _ = _CloudMqClientCtrl.SendMessageAsync(_GlobalMqTopics.RobotWheelStatusTopic, A);
                };


                //机器人移动到达事件
                _NavHelper.MoveStatusAction += (A, B) =>
                {
                    if (A)
                    {
                        Console.WriteLine($"[{DateTime.Now.ToString("yyyyMMdd HH:mm:ss:ff")}] robot move plan sucess.action_id:{B}");
                    }

                    //   this.SucessedAchievedEvent?.Invoke(A, B);
                };
            }


            ////机器人当前运行速度反馈
            //_NavHelper.RobotVelCallBack += (A, B, C) =>
            //{
            //    Speed = Math.Sqrt(A * A + B * B);
            //    RobotSpeedInfoCallBack?.Invoke(A, B, C);
            //};
        }
        #endregion

        #region 充电动作
        //public bool doChargeFunc(double speed, double chargedistance, int time)
        //{
        //    bool issucess = _NavHelper.StartCharge(speed, chargedistance, time).Result;

        //    return issucess;
        //}
        #endregion

        /// <summary>
        /// 设置下发速度
        /// </summary>
        public void SendMoveSpeed(double X, double Y, double Z)
        {
            _NavHelper.SendMoveSpeed(X, Z);
        }
        #endregion

        #region 设置电机升降
        /// <summary>
        /// 设置电机升降
        /// </summary>
        /// <param name="jsondata"></param>
        public async Task<(bool successed, string msg)> MotorDevManage(string jobNodeInfo)
        {
            JObject Senddata = new JObject();
            Senddata.Add("name", "LiftingEquipment");
            string data = jobNodeInfo;
            Senddata.Add("value", data);
            // Console.WriteLine("_RobotCtrlData   + Motor " + data);
            Console.WriteLine($"[{DateTime.Now.ToString("yyyyMMdd HH:mm:ss:ff")}]:_RobotCtrlData   + Motor {data}");
            _ = _LocalMqClientCtrl.SendMessageAsync(MqTopics.StackerLiftingCtrl_Client, Senddata.ToString());
            return (true, "sucessed");
        }
        #endregion

        #region 旋转电机控制
        /// <summary>
        /// 旋转电机控制
        /// </summary>
        /// <param name="jobNodeInfo"></param>
        public async Task<(bool successed, string msg)> SpinMotorDevManage(string jobNodeInfo)
        {
            JObject Senddata = new JObject();
            Senddata.Add("name", "RotationEquipment");
            string data = jobNodeInfo;
            ////string data = isOpen ? "on" : "off";
            Senddata.Add("value", data);
            Console.WriteLine($"[{DateTime.Now.ToString("yyyyMMdd HH:mm:ss:ff")}]:_RobotCtrlData   + SpinMotor {data}");
            _ = _LocalMqClientCtrl.SendMessageAsync(MqTopics.StackerLiftingCtrl_Client, Senddata.ToString());
            return (true, "sucessed");
        }
        /// <summary>
        /// 红灯控制
        /// </summary>
        /// <param name = "isOpen" ></ param >
        public void RedLightManage(bool isOpen)
        {
            JObject jsondata = new JObject();
            jsondata.Add("name", "RedLightEquipment");
            string data = isOpen ? "on" : "off";
            jsondata.Add("value", data);
            _ = _LocalMqClientCtrl.SendMessageAsync(MqTopics.StackerLiftingCtrl_Client, jsondata.ToString());
        }

        public async Task StartAsync(CancellationToken cancellationToken)
        {
            // InitNav();
            NavMsgDetailFunc();
            await AddSubMqFunc();
            MqMsgRecvEventFunc();
            // 服务初始化
            PubRobotJobStatusTimer = new Timer(async _ => await PubRobotJobStatusFunc(),
                    null, TimeSpan.FromSeconds(5), TimeSpan.FromSeconds(3));
        }

        public Task StopAsync(CancellationToken cancellationToken)
        {
            PubRobotJobStatusTimer?.Dispose();
            PubRobotJobStatusTimer = null;
            _CloudMqClientCtrl.Dispose();
            _LocalMqClientCtrl.Dispose();
            _NavHelper.Dispose();
            return Task.CompletedTask;
        }

        #endregion
    }



    //机器人工作状态 充电动作规划到任务中
    public enum RobotWorkStatusEnum
    {
        Running,   //工作中
        Charging,  //充电状态
        Free       //空闲
    }

}


